#include <iostream>
#include <string>
#include <chrono>
#include <thread>
#include <signal.h>

#include "serial_port.h"
#include "msp_protocol.h"
#include "msp_parser.h"
#include "attitude_visualizer.h"

// 全局标志，用于处理Ctrl+C信号
volatile bool running = true;

// 信号处理函数
void signalHandler(int signal) {
  if (signal == SIGINT || signal == SIGTERM) {
    std::cout << "接收到终止信号，正在退出..." << std::endl;
    running = false;
  }
}

// 帮助信息
void printHelp(const char *programName) {
  std::cout << "使用方法: " << programName << " [选项]" << std::endl;
  std::cout << "选项:" << std::endl;
  std::cout
      << "  -p, --port <串口>     指定串口设备 (默认: /dev/ttyUSB0 或 COM3)"
      << std::endl;
  std::cout << "  -b, --baud <波特率>   指定波特率 (默认: 115200)" << std::endl;
  std::cout << "  -h, --help            显示此帮助信息" << std::endl;
  std::cout << std::endl;
  std::cout << "示例:" << std::endl;
  std::cout << "  " << programName << " -p /dev/ttyACM0 -b 115200" << std::endl;
  std::cout << "  " << programName << " --port COM4 --baud 57600" << std::endl;
}

int main(int argc, char *argv[]) {
  // 设置信号处理
  signal(SIGINT, signalHandler);
  signal(SIGTERM, signalHandler);

  // 默认串口设置
  std::string portName;
  int baudRate = 115200;

  // 根据平台设置默认串口
#ifdef _WIN32
  portName = "COM3";
#else
  portName = "/dev/ttyACM0";
#endif

  // 解析命令行参数
  for (int i = 1; i < argc; i++) {
    std::string arg = argv[i];

    if (arg == "-h" || arg == "--help") {
      printHelp(argv[0]);
      return 0;
    } else if (arg == "-p" || arg == "--port") {
      if (i + 1 < argc) {
        portName = argv[++i];
      } else {
        std::cerr << "错误: -p/--port 选项需要一个参数" << std::endl;
        return 1;
      }
    } else if (arg == "-b" || arg == "--baud") {
      if (i + 1 < argc) {
        try {
          baudRate = std::stoi(argv[++i]);
        } catch (const std::exception &e) {
          std::cerr << "错误: 波特率必须是一个数字" << std::endl;
          return 1;
        }
      } else {
        std::cerr << "错误: -b/--baud 选项需要一个参数" << std::endl;
        return 1;
      }
    } else {
      std::cerr << "未知选项: " << arg << std::endl;
      printHelp(argv[0]);
      return 1;
    }
  }

  std::cout << "Betaflight 姿态可视化工具" << std::endl;
  std::cout << "正在连接到 " << portName << " (波特率: " << baudRate << ")"
            << std::endl;

  try {
    // 创建串口对象
    SerialPort serialPort;

    // 创建MSP解析器
    MSPParser mspParser;

    // 创建姿态可视化器
    AttitudeVisualizer visualizer;

    // 设置MSP解析器回调
    mspParser.setAttitudeCallback(
        [&visualizer](const MSP::AttitudeData &attitude) {
          visualizer.updateAttitude(attitude);
        });

    mspParser.setRawImuCallback([&visualizer](const MSP::RawImuData &imu) {
      visualizer.updateRawImu(imu);
    });

    // 设置串口数据回调
    serialPort.setDataCallback([&mspParser](const std::vector<uint8_t> &data) {
      mspParser.parse(data);
    });

    // 打开串口
    if (!serialPort.open(portName, baudRate)) {
      std::cerr << "无法打开串口 " << portName << std::endl;
      return 1;
    }

    // 启动串口异步读取
    serialPort.startReading();

    // 启动姿态可视化器
    visualizer.start();

    // 创建请求姿态数据的MSP帧
    MSP::Frame attitudeFrame = mspParser.createFrame(MSP::MSP_ATTITUDE);
    std::vector<uint8_t> attitudeRequest = attitudeFrame.serialize();

    // 创建请求IMU数据的MSP帧
    MSP::Frame imuFrame = mspParser.createFrame(MSP::MSP_RAW_IMU);
    std::vector<uint8_t> imuRequest = imuFrame.serialize();

    std::cout << "已连接到飞控，正在接收数据..." << std::endl;
    std::cout << "按Ctrl+C退出" << std::endl;

    // 主循环：定期请求数据
    while (running) {
      // 请求姿态数据
      serialPort.write(attitudeRequest);

      // 等待一小段时间
      std::this_thread::sleep_for(std::chrono::milliseconds(20));

      // 请求IMU数据
      serialPort.write(imuRequest);

      // 等待一段时间，避免请求过于频繁
      std::this_thread::sleep_for(std::chrono::milliseconds(80));
    }

    // 停止服务
    visualizer.stop();
    serialPort.stopReading();
    serialPort.close();

    std::cout << "程序已退出" << std::endl;
  } catch (const std::exception &e) {
    std::cerr << "错误: " << e.what() << std::endl;
    return 1;
  }

  return 0;
}
